# -*- coding: utf-8 -*-

from __future__ import print_function

import glob
import os
import sys
# import time

try:
    sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass
import carla
from carla import Transform, Location, Rotation
# import random
# import open3d
# from agents.navigation.behavior_agent import BehaviorAgent

def main():
    actor_list = []
    sensor_list = []

    try:
        # 设置世界
        client = carla.Client('localhost', 2000)
        client.set_timeout(2.0)
        world = client.get_world()

        weather = carla.WeatherParameters(sun_altitude_angle=70, cloudiness=10.0)
        world.set_weather(weather)
        # # 时间同步
        # origin_settings = world.get_settings()
        # settings = world.get_settings()
        # settings.synchronous_mode = True
        # settings.fixed_delta_seconds = 0.05
        # world.apply_settings(settings)

        blueprint_library = world.get_blueprint_library()

        # 创建车辆
        ego_vehicle_bp = blueprint_library.find('vehicle.audi.a2')
        ego_vehicle_bp.set_attribute('color', '0,0,0')
        # 确定起点和终点
        vehicle_trans2 = Transform(Location(x=240.45972426616333, y=-150.58177141749717, z=2.0),
                                        Rotation(pitch=0.000000, yaw=0, roll=0.000000))
        # start_point = random.choice(world.get_map().get_spawn_points())
        print("位置设定完成")
        vehicle = world.spawn_actor(ego_vehicle_bp, vehicle_trans2)
        print("添加成功")
        # 设定为自动驾驶模式
        vehicle.set_autopilot(True)

        actor_list.append(vehicle)
        print("自动驾驶车辆设定完成")
        # 设置储存位置
        output_path = '../outputs/output_lidar'
        if not os.path.exists(output_path):
            os.makedirs(output_path)

        # 创建lidar，设置lidar的参数
        lidar_bp = blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp.set_attribute('channels', str(32))
        lidar_bp.set_attribute('points_per_second', str(90000))
        lidar_bp.set_attribute('rotation_frequency', str(10))
        lidar_bp.set_attribute('range', str(50))
        # 设置lidar架设的点
        lidar_location = carla.Location(-118, 24, 10)
        lidar_rotation = carla.Rotation(0, 0, 0)
        lidar_transform = carla.Transform(lidar_location, lidar_rotation)
        print("lidar设定完成")
        # 生成lidar并采集数据
        lidar = world.spawn_actor(lidar_bp, lidar_transform)
        print("lidar添加成功")
        lidar.listen(
            lambda point_cloud: point_cloud.save_to_disk(os.path.join(output_path, '%06d.ply' % point_cloud.frame)))
        sensor_list.append(lidar)
        print("lidar数据保存成功")

        # world.tick()

        # # 设置车辆的驾驶模式
        # agent = BehaviorAgent(vehicle, behavior='normal')
        # # 核心函数
        # agent.set_destination(agent.vehicle.get_location(), end_point.location, clean=True)



        while True:
            # world.tick()
            spectator = world.get_spectator()
            # transform = vehicle.get_transform()
            # spectator.set_transform(carla.Transform(transform.location + carla.Location(z=30),
            #                                         carla.Rotation(pitch=-90)))
        #     agent.update_information(vehicle)
        #
        #     world.tick()
        #
        #     if len(agent._local_planner.waypoints_queue) < 1:
        #         print('======== Success, Arrivied at Target Point!')
        #         break
        #
        #     # 设置速度限制
        #     speed_limit = vehicle.get_speed_limit()
        #     agent.get_local_planner().set_speed(speed_limit)
        #
        # control = agent.run_step(debug=True)
        # vehicle.apply_control(control)

    finally:
        # world.apply_settings(origin_settings)
        print('destroying actors')
        client.apply_batch([carla.command.DestroyActor(x) for x in actor_list])
        for sensor in sensor_list:
            sensor.destroy()
        print('done.')


if __name__ == '__main__':
    try:
        main()
    except KeyboardInterrupt:
        print(' - Exited by user.')